#include "mpu6050.h"
#include "port_i2c.h"
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "port_exit.h"

static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
#define q30  1073741824.0f

static T_Mpu6050 g_mpu6050;  
static T_Mpu6050 *p_mpu6050 = NULL;                                           
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;                                                                     
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];

static xQueueHandle mpu_queue = NULL;

int mpu_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{
    return i2c_write_bytes(MPU6050_I2C_PORT, slave_addr, reg_addr, (uint8_t*)data, length);
}

int mpu_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
    return i2c_read_bytes(MPU6050_I2C_PORT, slave_addr, reg_addr, (uint8_t*) data, length);
}

void mpu_exit_cbk(void *pv)
{
    uint32_t gpio_num = (uint32_t) pv;
    xQueueSendFromISR(mpu_queue, &gpio_num, NULL);
}

void mpu6050_init(void)
{
    i2c_init(MPU6050_I2C_PORT);
    mpu_queue = xQueueCreate(10, sizeof(uint32_t));
    exit_init(MPU6050_EXIT_PORT, mpu_exit_cbk, EXIT_INTR_NEGEDGE);

    uint8_t who_am_i = 0;
    mpu_i2c_read(0x68, 0x75, 1, &who_am_i);
    printf("who_am_i: 0x%X\n", who_am_i);

    int result;
    result = mpu_init();
    p_mpu6050 = (T_Mpu6050*)&g_mpu6050;
    if(!result)
    {	 		 
        mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
        
        mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
        
        mpu_set_sample_rate(DEFAULT_MPU_HZ); //mpu_set_sample_rate
        
        dmp_load_motion_driver_firmware(); //dmp_load_motion_driver_firmvare
        
        dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); //dmp_set_orientation

        
        dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
            DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
            DMP_FEATURE_GYRO_CAL);		   	  //dmp_enable_feature

        
        dmp_set_fifo_rate(DEFAULT_MPU_HZ);   	  //dmp_set_fifo_rate

        
        run_self_test();
        mpu_set_dmp_state(1);
    }
}

void mpu6050_update(void)
{   
    dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);

    /* Gyro and accel data are written to the FIFO by the DMP in chip
    * frame and hardware units. This behavior is convenient because it
    * keeps the gyro and accel outputs of dmp_read_fifo and
    * mpu_read_fifo consistent.
    */
			

    if (sensors & INV_XYZ_GYRO )
    {
        p_mpu6050->gyro.x = gyro[0];
        p_mpu6050->gyro.y = gyro[1];
        p_mpu6050->gyro.z = gyro[2];
    }
    if (sensors & INV_XYZ_ACCEL)
    {
        p_mpu6050->accel.x = accel[0];
        p_mpu6050->accel.y = accel[1];
        p_mpu6050->accel.z = accel[2]; 
    }

    /*Unlike gyro and accel, quaternions are written to the FIFO in
    * the body frame, q30. The orientation is set by the scalar passed
    * to dmp_set_orientation during initialization.
    */
	if (sensors & INV_WXYZ_QUAT )
    {
		q0=quat[0] / q30;	
		q1=quat[1] / q30;
		q2=quat[2] / q30;
		q3=quat[3] / q30;
        
		p_mpu6050->euler.roll = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3f; // roll
		p_mpu6050->euler.pitch  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f; //pitch 
		p_mpu6050->euler.yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3f;    
    }
}

void mpu_display(void)
{
    printf("roll=%.2f pitch=%.2f yaw=%.2f", p_mpu6050->euler.roll, p_mpu6050->euler.pitch, p_mpu6050->euler.yaw);
}

void mpu6050_task_example(void* arg)
{
    uint32_t io_num;
    printf("mpu6050_task_example\n");
    for(;;) {
        if(xQueueReceive(mpu_queue, &io_num, portMAX_DELAY)) 
        {
            mpu6050_update();
            mpu_display();
            printf("GPIO[%d] intr, val: %d\n", io_num, gpio_get_level(io_num));
        }
    }
}

T_Mpu6050 *mpu6050(void)
{
    return &g_mpu6050;
}



